import os
from collections import deque

import numpy as np
import math
EARTH_RADIUS_EQUA = 6378137.0


DEBUG = int(os.environ.get('HAS_DISPLAY', 0))


class Plotter(object):
	def __init__(self, size):
		self.size = size
		self.clear()
		self.title = str(self.size)

	def clear(self):
		from PIL import Image, ImageDraw

		self.img = Image.fromarray(np.zeros((self.size, self.size, 3), dtype=np.uint8))
		self.draw = ImageDraw.Draw(self.img)

	def dot(self, pos, node, color=(255, 255, 255), r=2):
		x, y = 5.5 * (pos - node)
		x += self.size / 2
		y += self.size / 2

		self.draw.ellipse((x-r, y-r, x+r, y+r), color)

	def show(self):
		if not DEBUG:
			return

		import cv2

		cv2.imshow(self.title, cv2.cvtColor(np.array(self.img), cv2.COLOR_BGR2RGB))
		cv2.waitKey(1)


class RoutePlanner(object):
	def __init__(self, min_distance, max_distance, debug_size=256, lat_ref=42.0, lon_ref=2.0):
		self.route = deque()
		self.min_distance = min_distance
		self.max_distance = max_distance

		# self.mean = np.array([49.0, 8.0]) # for carla 9.9
		# self.scale = np.array([111324.60662786, 73032.1570362]) # for carla 9.9
		self.mean = np.array([0.0, 0.0]) # for carla 9.10
		self.scale = np.array([111324.60662786, 111319.490945]) # for carla 9.10

		self.debug = Plotter(debug_size)
		# self.lat_ref, self.lon_ref = self._get_latlon_ref()
		self.lat_ref = lat_ref
		self.lon_ref = lon_ref

	def set_route(self, global_plan, gps=False, global_plan_world = None):
		self.route.clear()

		if global_plan_world:
			for (pos, cmd), (pos_word, _ )in zip(global_plan, global_plan_world):
				if gps:
					pos = self.gps_to_location(np.array([pos['lat'], pos['lon']]))
					# pos -= self.mean
					# pos *= self.scale
				else:
					pos = np.array([pos.location.x, pos.location.y])
					# pos -= self.mean
				
				self.route.append((pos, cmd, pos_word))
		else:
			for pos, cmd in global_plan:
				if gps:
					pos = self.gps_to_location(np.array([pos['lat'], pos['lon']]))
					# pos -= self.mean
					# pos *= self.scale
				else:
					pos = np.array([pos.location.x, pos.location.y])
					# pos -= self.mean

				self.route.append((pos, cmd))

	def run_step(self, gps):
		self.debug.clear()

		if len(self.route) == 1:
			return self.route[0]

		to_pop = 0
		farthest_in_range = -np.inf
		cumulative_distance = 0.0

		for i in range(1, len(self.route)):
			if cumulative_distance > self.max_distance:
				break

			cumulative_distance += np.linalg.norm(self.route[i][0] - self.route[i-1][0])
			distance = np.linalg.norm(self.route[i][0] - gps)

			if distance <= self.min_distance and distance > farthest_in_range:
				farthest_in_range = distance
				to_pop = i

			r = 255 * int(distance > self.min_distance)
			g = 255 * int(self.route[i][1].value == 4)
			b = 255
			self.debug.dot(gps, self.route[i][0], (r, g, b))

		for _ in range(to_pop):
			if len(self.route) > 2:
				self.route.popleft()

		self.debug.dot(gps, self.route[0][0], (0, 255, 0))
		self.debug.dot(gps, self.route[1][0], (255, 0, 0))
		self.debug.dot(gps, gps, (0, 0, 255))
		self.debug.show()

		return self.route[1]
	
	def gps_to_location(self, gps):
		# gps content: numpy array: [lat, lon, alt]
		lat, lon = gps
		scale = math.cos(self.lat_ref * math.pi / 180.0)
		my = math.log(math.tan((lat+90) * math.pi / 360.0)) * (EARTH_RADIUS_EQUA * scale)
		mx = (lon * (math.pi * EARTH_RADIUS_EQUA * scale)) / 180.0
		y = scale * EARTH_RADIUS_EQUA * math.log(math.tan((90.0 + self.lat_ref) * math.pi / 360.0)) - my
		x = mx - scale * self.lon_ref * math.pi * EARTH_RADIUS_EQUA / 180.0
		return np.array([x, y])